import sympy
import math
import numpy as np
import matplotlib
import matplotlib.pyplot as plt
import csv
import os
from decimal import *
pre_yaw_deg = 0
pre_yaw_rad = pre_yaw_deg/180.0*math.pi
print(os.path.dirname(os.path.realpath(__file__)))
path = os.path.dirname(os.path.realpath(__file__))+'/../data/'
f = open(path+'dlc.csv', 'w')
dlc_writer = csv.writer(f)
dlc_writer.writerow(["x", "y", "theta", "kappa", "v", "s", "a"])

x = sympy.symbols('x')
z1 = 2.4/25.0*(x-27.0)-1.2  # x-10 for 13gong
z2 = 2.4/25.0*(x-54.0)-1.2  # x-25 for 13gong
dx1 = 25.0
dx2 = 25.0
dy1 = 5.0
dy2 = 5.0
y = dy1/2*(1+sympy.tanh(z1))-dy2/2*(1+sympy.tanh(z2))
# yaw = sympy.atan(dy1*sympy.cosh(z1)**(-2)*(1.2/dx1) -
#                 dy2*sympy.cosh(z2)**(-2)*(1.2/dx2))
yaw_calc = sympy.atan(sympy.diff(y, x))
der_1st = sympy.diff(y, x)
der_2nd = sympy.diff(der_1st, x)
kappa = der_2nd/((1+der_1st**2)**(1.5))
ds = (1+der_1st**2)**(1/2)
y_list = []
yaw_list = []
yaw_calc_list = []
kappa_list = []
s_list = []


start_length = 80
ex_x_np = np.arange(0, start_length, 0.25)
for ex_x in ex_x_np:
    temp_x = ex_x
    temp_y = 0
    temp_yaw = 0
    temp_x1 = temp_x*math.cos(pre_yaw_rad)-temp_y*math.sin(pre_yaw_rad)
    temp_y1 = temp_x*math.sin(pre_yaw_rad)+temp_y*math.cos(pre_yaw_rad)
    temp_yaw1 = temp_yaw+pre_yaw_deg
    dlc_writer.writerow(
        [str(Decimal.from_float(temp_x1).quantize(Decimal('0.000000'))),
         str(Decimal.from_float(temp_y1).quantize(Decimal('0.000000'))),
         str(Decimal.from_float(temp_yaw1).quantize(Decimal('0.000000'))),
         str(Decimal.from_float(0).quantize(Decimal('0.000000'))),
         10.0,
         str(Decimal.from_float(ex_x).quantize(Decimal('0.000000'))),
         0.0])

mid_length = 120
x_np = np.arange(0, mid_length, 0.25)
for x_value in x_np:
    temp_y = float(y.evalf(subs={'x': x_value}))
    y_list.append(temp_y)
    # temp_yaw = yaw.evalf(subs={'x': x})
    # yaw_list.append(temp_yaw)
    temp_yaw_calc = float(yaw_calc.evalf(subs={'x': x_value}))*180/3.14159265
    yaw_calc_list.append(temp_yaw_calc)
    temp_kappa = float(kappa.evalf(subs={'x': x_value}))
    kappa_list.append(temp_kappa)
    temp_s = float(sympy.integrate(ds, (x, 0, x_value)))
    s_list.append(temp_s)

    temp_x = x_value + start_length
    temp_y = temp_y
    temp_yaw = temp_yaw_calc

    temp_x1 = temp_x*math.cos(pre_yaw_rad)-temp_y*math.sin(pre_yaw_rad)
    temp_y1 = temp_x*math.sin(pre_yaw_rad)+temp_y*math.cos(pre_yaw_rad)
    temp_yaw1 = temp_yaw+pre_yaw_deg
    dlc_writer.writerow(
        [str(Decimal.from_float(temp_x1).quantize(Decimal('0.000000'))),
         str(Decimal.from_float(temp_y1).quantize(Decimal('0.000000'))),
         str(Decimal.from_float(temp_yaw1).quantize(Decimal('0.000000'))),
         str(Decimal.from_float(temp_kappa).quantize(Decimal('0.000000'))),
         10.0,
         str(Decimal.from_float(temp_s+start_length).quantize(Decimal('0.000000'))),
         0.0])

end_length = 20
start_s = s_list[-1]
ex_x_np = np.arange(start_length+mid_length, start_length +
                    mid_length + end_length, 0.25)
for ex_x in ex_x_np:
    temp_x = ex_x
    temp_y = 0
    temp_yaw = 0

    temp_x1 = temp_x*math.cos(pre_yaw_rad)-temp_y*math.sin(pre_yaw_rad)
    temp_y1 = temp_x*math.sin(pre_yaw_rad)+temp_y*math.cos(pre_yaw_rad)
    temp_yaw1 = temp_yaw+pre_yaw_deg

    dlc_writer.writerow(
        [str(Decimal.from_float(temp_x1).quantize(Decimal('0.000000'))),
         str(Decimal.from_float(temp_y1).quantize(Decimal('0.000000'))),
         str(Decimal.from_float(temp_yaw1).quantize(Decimal('0.000000'))),
         str(Decimal.from_float(0).quantize(Decimal('0.000000'))),
         10.0,
         str(Decimal.from_float(
             ex_x-mid_length+start_s).quantize(Decimal('0.000000'))),
         0.0])

y_np = np.array(y_list)
# yaw_np = np.array(yaw_list)
yaw_calc_np = np.array(yaw_calc_list)
kappa_np = np.array(kappa_list)
s_np = np.array(s_list)
fig, ax = plt.subplots(4, 1)
ax[0].plot(x_np, y_np)
# ax[1].plot(x_np, yaw_np)
ax[1].plot(x_np, yaw_calc_np)
ax[2].plot(x_np, kappa_np)
ax[3].plot(x_np, s_np)
plt.show()
